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Abstract 

We study the problem of high accuracy localization of mobile nodes in a multipath-rich environment where 
sub-meter accuracies are required. We employ a peer-to-peer framework where the vehicles/nodes can get pairwise 
multipath-degraded ranging estimates in local neighborhoods. The challenge is to overcome the multipath-barrier 
using redundancy in measurements across time and space enabled through cooperation, in order to provide the 
desired accuracies especially under severe multipath conditions when the fraction of received signals corrupted 
by multipath is dominating. We invoke an analytical graphical model framework based on particle filtering and 
validate its potential for high accuracy localization through simulations. We also address design questions such as 
"How many anchors and what fraction of line-of-sight (LOS) measurements are needed to achieve a specified target 
accuracy?", by showing that the Cramer-Rao Lower Bound for localization can be expressed as a product of two 
factors - a scalar function that depends only on the parameters of the noise distribution and a matrix that depends 
only on the geometry of node locations and the underlying connectivity graph. A simplified expression is obtained 
for the CRLB, which provides an insightful understanding of the bound and helps deduce the scaling behavior of 
the estimation error as a function of the number of agents and anchors in the network. 

Keywords - Localization, Multipath, Graphical Models, Particle Filtering, Cramer Rao Bound. 

L Introduction 

High- accuracy localization is mandated in many applications like vehicle safety |T|, autonomous robotic systems 
0, Unmanned Air Vehicle (UAV) systems etc, where sub-meter accuracies are needed. Standard GPS receivers 
can have localization errors of over fifty or more meters which is unacceptable for many of these applications. 
The principal problem is multipath interference |[3|, which is particularly prevalent in cities and "urban canyon" 
environments, and corrupts a large fraction of the measurements. Many of the existing solutions such as D-GPS, 
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Figure 1. Peer-to-Peer Collaborative Localization 

A-GPS, N-RTK ||4| etc, that augment the GPS system are typically expensive and further fail to address multipath. 
Our goal here is to design algorithms that can provide sub-meter accuracies in severe multipath environments with 
minimal communication overhead given the bandwidth constraint in applications such as vehicular safety enabled 
by technology like DSRC (Dedicated Short Range Communication) ||5|. Our design philosophy is a "peer to peer" 
architecture (see Fig [T]) where nodes collaborate and help each other refine their position estimates. Collaboration 
coupled with mobility generates a large pool of measurements in the system. The fundamental insight is that, some 
fraction of these measurements will be produced by line-of-sight (LOS) dominated signals, and hence be fairly 
accurate, while some fraction will be corrupted by dominated non-line-of-sight (NLOS) reflected waves. Receivers 
do not know a priori which measurements are LOS and which are NLOS. Hence, the task of the users is to 
cooperatively discard the NLOS signals, thus enabling them to compute high-precision position estimates. Our first 
main contribution in this paper is to uncover a framework and a distributed algorithm founded on graphical models 
for collaborative narrowband NLOS localization with minimal messaging overhead. The proposed algorithm shows 
promise of sub-meter accuracies in a realistic simulation setup. 

Our second main contribution in this paper is a theoretical characterization of the localization accuracy as 
a function of the number of anchors (nodes with known locations), agents/vehicles and the fraction of LOS 
measurements by analyzing the Cramer-Rao Lower Bound (CRLB) Q a fundamental lower bound on the best 
possible mean square error achievable using an unbiased estimator. For a generalized distance/angle measurement 
model, we provide a simplified characterization of the scaling behavior of the bound as a function of the number 
of nodes in the network and the fraction of LOS measurements. Further we show a "separation principle" under 
simplifying assumptions, wherein the effect of the node geometry and the noise distribution can be independently 
analyzed. 

^The CRLB is mostly tight in the high signal-to-noise (SNR) ratio regime and is not a good indicator of the estimator performance at low 
SNR. Other bounds like the Ziv-Zakai and Barankin bounds give a good characterization at low SNR but are quite complex to offer any 
insights. 
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The paper is organized as follows. Section II discusses the related work in this area and the positioning of our 
work in this literature. Section III sets up the problem formulation and notations. Section IV describes our inference 
algorithm for NLOS localization. Section V summarizes our theoretical results and bounds on the localization 
accuracy. Section VI provides simulation results comparing the performance of the algorithm with the derived 
theoretical bounds. 

II. Related work 

There has been significant research work focusing on cooperative localization algorithms particularly for the LOS 
case ||6|-|[8|. However NLOS localization has received relatively lesser focus in the literature. There has been quite 
some work on non-cooperative (i.e. no collaboration between mobile agents) NLOS localization ||3|, |[9|, [ [T0| . A 



major part of this literature focuses on consensus based RANSAC (RAndom SAmple Consensus) like algorithms 



[ ^0| which in effect assigns a 0-1 weight to the measurements detecting them to be LOS or NLOS. The complexity 
of this combinatorial approach explodes with increasing fraction of NLOS measurements especially in the mobile 
setting. Our algorithm can be conceptually thought of assigning a "soft- weight" to the measurements [ [TT| , [ [T2| 



which helps get the complexity under control and further exploit the statistics of the measurements. 

There is also some work on using ultra-wide-band (UWB) [ [T3| or multi-antenna array systems [[T4|-[[T6| for 



localization where different arriving paths can be distinguished. Wymeersch et al. p3] and consequent follow-up 
works [ |T7| propose a graphical model framework for the problem of interest which though has some similarities to 
our work, is quite different in the following aspects. We focus on narrowband environments leading to a different 
noise model that is relevant in vehicular safety applications. We also model correlation of readings across time that 



arises in the vehicular setup p8] | and our algorithm efficiently handles this while existing work does not consider 
correlation. Further, our concern is also towards minimizing the communication overhead and our algorithm only 
requires a minor overhead to the existing DSRC message that needs to be transmitted periodically every 100ms as 
prescribed by the standard. There is more recent work focusing on convex optimization frameworks for the problem 
of interest [ [T9| which are analytically tractable but computationally hard and communication intensive especially 
in mobile environments. 



There is considerable theoretical work addressing fundamental performance bounds for LOS localization (6), (20)- 
[ [23| . However, the complex nature of the expressions renders it difficult to gain insights on its scaling behavior as a 
function of the number of nodes. Further, there is relatively less literature for the NLOS setting p4| , p5| . Shen et 



al. p5[ | analyze the CRLB for UWB cooperative localization. Qi et al. [24| consider a narrowband non-cooperative 
case with assumption on the exact knowledge of which measurements are NLOS that is difficult to get in practice 
especially in narrowband environments which is the focus of this work. In practice, it might be reasonable to assume 
that one has prior knowledge of the statistics of the NLOS distribution. For example, exponential noise models for 
NLOS have been proposed in the literature [ [26| . An analytical characterization of the performance as a function of 
the NLOS noise distribution and the fraction of LOS measurements is missing in the literature, which we address 
here. One of our contributions is to provide concrete insights into the scaling behavior of the bound as a function 
of the number of anchors, vehicles and the fraction of LOS measurements. An arxiv p7| version of this paper 



contains detailed proofs and descriptions. Parts of this work have appeared in conference proceedings |28|, p9|. 



in. Problem Setup 

We consider a network of N mobile agents and M static anchors with known locations (see Fig.[T]). Let ut{k) G C 
denote the true location of the kth vehicle at time instant t, where the real part represents the x-coordinate and 
the imaginary part the ^/-coordinate. Measurements of the form 9t{km) = f{ut{k)^ut{m)) -\-nt{km)^ are obtained 
between vehicles that are within a communication radius R. The function /(.) depends on the measurement modality. 
For example, f {ut{k) ^ ut{m)) = \ \ut{k) —ut{m)\ \ in the case of Time of Arrival based sensors, f{ut{k)^ut{m)) oc 
\\ut{k)-u,{m)\\'y ^'^^ ^^^^ Received Signal Strength measurements, f{ut{k),ut{m)) = tan-^ i^(n,\^fe)-r'(m)) 
for Angle of Arrival measurements etc. Each measurement is modeled as either a LOS dominated signal or a NLOS 
dominated signal by choosing the observation noise in the received signal nt{km), to be drawn from a mixture of two 
distributions, {pLOs{Ot{km)\ut{k)^ut{m))^PNLOs{0t{krn)\ut{k),ut{rn)))), with mixture probabilities (a, 1 — a) 
respectively. The model is motivated by some of the experimental work carried out in the UWB p0|-[[32| which 
validate that some fraction of the received signals are purely LOS-dominated signals. Let zt{km) be an indicator 
random variable which is 1 if 9t{km) ^ pLOs{^t{km)\ut{k)^ut{m))^ otherwise. zt{km) can be correlated in 
time (e.g. an obstruction between two vehicles could lead to sustained NLOS condition) which we capture using 
a Markov chain with stationary distribution (a, 1 — a). Let p{ut{k)\ut-i{k)) be the distribution that governs the 
evolution of the vehicle states across time which is obtained based on the inertial navigation system measurements. 



The goal is for each vehicle k to estimate its own location ut{k), based on all measurements {9r{km)y^^i from 
its neighbors upto time t. Given the non-gaussian nature of the problem, we adopt a particle filtering approach that 
is a popular Monte-Carlo technique which can provide accuracies close to Minimum Mean Square Error (MMSE) 
estimates. The nature of our problem helps us obtain Kalman-like updates for particle filtering giving rise to a 
simplified and practical algorithm. We describe the graphical model formulation and our algorithm in the next 
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Figure 2. Graphical model representation of the unknown vehicle states (locations) and the observations (LOS/NLOS measurements), 
section. 

IV. Graphical models & Particle Filtering for Cooperative Localization 

Graphical models provide a good way of modeling dependencies between different random variables [ [33| and 
thereby exploit the probability structure to develop computationally feasible estimation algorithms. A directed acyclic 
graphical model, ^(V, £), consists of a vertex set V where each vertex is associated with a random variable and an 
edge set £ which is the collection of all directed edges, with the conditional independencies encapsulated by graph 
separation [ [33| . Dependencies amongst the vehicle locations and the readings are captured by the graphical model 
shown in Fig [2j which is a coupled Hidden Markov Model (HMM). The unshaded nodes are the hidden nodes to 
be estimated |^ and the shaded nodes are observations coupling the different Markov chains of the vehicles, termed 
as evidence nodes. The joint probability distribution of all the random variables is given by, 

N 

p{{ut}AOt}A^t]) = n V{0t{km)\ut{k),ut{m),zt{km))\{p{^^^^ (1) 

t,k,m k=l t 

Y[p{zi{km))Y[p{zt{km)\zt-i{km)). (2) 

k,m t 

Celebrated algorithms like the loopy belief propagation p3| are hard to apply here given the continuous value 
of the state space and the dynamic evolving nature of the graph. 

Particle Filtering is a well-known Monte Carlo simulation technique |34| whose goal is to approximate the 
posterior state density that can be used to obtain the MMSE estimate. Exact inference over Fig [2] being hard, we 

^Anchor nodes are not shown in this model for simplicity. 



'"^Estimated vehicle locations 



Figure 3. Approximation for the original coupled HMM. 



resort to an approximation for every vehicle as shown in Fig [3] The new set of shaded nodes in this graph correspond 



to the estimated location of the other vehicles. At every time instant, each vehicle gets the estimated location of 
its neighbors from the previous time instants and assuming that it is close enough to the true location, the vehicle 
gets an estimate of its own location using particle filtering. This also reduces the communication overhead between 
vehicles and each vehicle needs to only transmit its location estimate and possibly the variance. Treating {ut^zt} 
as a single state would lead to a exponentially large state space with increasing neighbors. We describe an efficient 
algorithm to circumvent this issue. 

Consider only a single vehicle for now and omit the vehicle index k for notational simplicity. Let {ui:t^ zi:t} and 
6i:t denote the set of states and observations up to time t respectively. The posterior density of the vehicle location 
given the observations can be approximated as p{ut\Oi:t) ^ p{ut\Oi:t) = -k^i^i^i^t — ^1) where {ul;i = 
1,...,K} are K i.i.d random samples (particles) picked from the distribution p{ut\9i:t) and S{.) is the dirac- 
delta function. Given the hardness of sampling from p{ut\9i:t) , we choose a proposal distribution 7T{ui:t\9i:t) = 
^{ui:t-i\0i:t-i)7r{ut\ui:t-i^ Oi:t) . Supposc that wc are interested in estimating the mean of some function, f{ui-t) 
of the vehicle location across time, i.e. E(/(i/i:t)). We then have the following: 




(3) 



IE7r(/(^l:t)^t(^l:t)) 



(4) 



where wt{ui:t) 



p{Ol:t\Ul:t)p{Ul:t) 
7T{Ui:t\0i:t) 



E. 



p{Ol:t,Zt\ui:t)p{ui:t) 
7T{Ui:t\0i..t) 



. Let, 



p{Ol:t,Zt\ui:t)p{ui:t) 
7T{ui:t\0i:t) 



(5) 



We can now obtain a recursive update equation for </>(.) as follows. 



Kuv.t^zt) = ^— , (6) 

T:{uv.t\Ol:t) ' ^ ^ 

Zt-1 

E p{zt,zt-i\ui;t)p{9i:t-i\zt-i,ui;t-i)p{9t\zt,ut) 

T^{ui:t\dl..t) ^ ^ 

Zt-1 

_ p{zt, zt-i)p{9i,t-i\zt-i,ui,t-i)p{0t\zt, ut) 



Zt-1 



p{9t\zt,ut)p{ut\ut- 



7v{ut\ui: 



^ ^^p{zt\zt-i)(t){ui.,t-i,zt-i), (10) 

Zt-1 

where the last three equahties follow from the conditional independence structure of the different random variables 
(i.e. zt^ are unconditionally independent of ut^ and 9t only depends on zt and ut). 
Choosing Ti{ut\ui:t-i9i;t) = p{ut\ut-i), we get, 

0(^X1:^, Zt) = p{Ot\zt, Ut) ^p{zt\zt-l)(l){ui:t-l, Zt-l) . (11) 

Zt-1 

For each vehicle k and its neighbor m we have (j)t{km) and the update equations are given in Algorithm 1. All the 
expectations in the above equations are replaced by summations over samples of ul taken from the chosen proposal 
distribution. To take care of degeneracy issues over long time instants p4| , we employ the standard resampling 
procedure whenever the number of distinct particles fall below a threshold. In our simulations in Section |Vl| we 
had 2000 particles and whenever the effective sample size calculated as (A^e// = ^ {w^y ) was below a threshold 
(30 here), a simple multinomial resampling was carried out. If the number of distinct particles was too low, then 
all the particles were newly sampled around the estimated position. 



Conceptually, the performance of the algorithm depends on different system parameters such as the noise in 
the ranging measurements, INS noise, number of anchors, number of neighbors etc. In particular, the algorithm 
banks on the INS reading to be good enough to sustain the vehicle location until enough LOS measurements have 
been obtained. The algorithm is more sensitive to INS noise than typical belief propagation algorithms given that 
each vehicle extrapolates the neighbor's location estimate to the current time instant using the INS reading and 
there is no iteration to improve this estimate at that time instant. Hence large errors in INS readings would lead 
to significant performance degradation. The hope is that the INS measurements in existing vehicles can be quite 
precise for the algorithm to work well p5| . Further the LOS measurements are assumed to be quite precise and 
the number of neighbors is assumed to be large and well spread. Failure of any or some of these conditions would 
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Algorithm 1 Particle Filtering for Accurate Localization 

1: Initialize: Sample {ito(^)}i^i from p(ito(/c)), set WQ^k) = V/c. 
2: for t > 1 do 

3: Sample It J (/c) from p{ut{k)\ul_^{k)). 

4: for all Neighbors m do 

5 : if m was a neighbor at t — 1 then 

6: (t>\{km) ^ p{et{km)\zt{km),u\{k)) 

^zt_i P(zt(km)\zt-i(km))(l)l_^(km). 

7: else 

8: (t)\{kra) = p{zt{km))p{9t{km)\u\{k), zt{km)). 

9: end if 

10: end for 

11: Calculate wi{k) = Urr. E.,(/cm) 0t(^^) 
12: Normalize it; J and resample. 
13: end for 

14: Output:£:[iit(/e)|{6>i:t(/cm)}] = wi{km)ui{k). 




(a) Good node geometry and (b) Bad underlying (c) Bad node geonnetry 

connectivity graph. connectivity graph 



Figure 4. Examples of node geometries and connectivity graphs. 



lead to performance degradations. Extensive simulation results under a realistic setup are presented in Section VI 



V. Scaling Laws for Cooperative NLOS localization 

From an engineering design perspective, it is important to understand the effect of the node geometry, connectivity 
graph, number of agents and anchors and the fraction of LOS measurements, on the localization performance. We 
modestly attempt to analyze these effects in a simple static setup with N agents and M anchors placed in a two- 
dimensional field. Let u = {u(k)} is the vector of all agent locations and u the estimated locations. We study the 
the behavior of the Cramer-Rao Lower Bound (CRLB), which is a lower bound on the estimation error for the 



class of unbiased estimators (i.e. E(u) = u, where E is the expectation operator). Let 77 = ur;ui ^ where 
ur = i?e{u},ui = /m{u}, be the vector of parameters to be estimated. The Cramer-Rao theorem states that, 
E[(u — u)(u — u)*] y where a* represents conjugate transpose of a complex column vector a and the matrix 
F, known as the Fisher Information Matrix, is defined by, Fkm = ^ { ^^""l^^'"^^ ^^^ry?'"^^ } • 
^We use the notation [a; b] to represent the vertical concatenation of two column vectors a and b. 



The errors in localization can be attributed to two factors, one being the relative geometry of the anchors and 
vehicle locations (see Fig. |4]) and the other being the noise in the measurements. Thus one can naturally ask the 
question, "Can the effect of node geometry and that of noise be independently analyzed?". We answer this question in 
the affirmative under certain simplifying assumptions of homogenous noise statistics. Our theory is developed for the 
measurement model in Section |ni| with the further assumption that all the readings are independent and identically 
distributed. Even though for a fixed realization, each vehicle might experience different fading environments, when 
averaged across multiple deployments of the vehicles and scatterers and time, it is reasonable to assume that the 
statistics of the noise is similar across the network. Further, this simplifying assumption helps us gain intuition 
on how the different system parameters affect the performance of the system. The following theorem states the 
separation under the above conditions. 

Theorem 1 (Separation Principle:). The Fisher Information Matrix can be written as F — g{pNOiSE)FGy where 
the matrix Fq depends only on the node locations and the underlying connectivity graph. The scalar function g(.) 
is given by, g{pNOiSE) = E | In Pnoise^^))^^ , under the assumption that Pnoise is differentiable over its 
support [LL, UL] and p{UL) — p{LL) = 0. 

Proof: For simplicity, let us focus on the case where 6{km) = \ \u{k) —u{m)\ \ -^n{km), though the derivation 
extends to the general case. The (A:,m)th entry in the matrix is given by, Fkm = { ^^""iTif ^^^rlf'"^^ } • Let 
J\f{k) be the set of neighbors of node k. Let us focus on the case when r]k = UR{k) and 77^ = UR{m). It is easy 
to show that, 

d\np{e\r]) d\npNOiSE{n{ki)) ^^^z ^ ^^2) 

where n{ki) = e{ki) - \ \u{k) - u{i)\\ and cos(0/ei) = (note that cos{(j)ki) = -cos{(l)ik) )• Thus we 



have, 



= E < 
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Consider the case when k = m. We get that, 



Fkk 



E< 



^ COS^ {(f)ki) 



9 {p noise) 



p'{n{ki)) 
p{n{ki)) 



+ 



^ COs{(j)ki) COs{(j)kj) 



(i^j)eJ\f(k) 



p' {n{ki)) p' {n{kj)) 
p{n{ki)) p{n{kj)) 



^ cos^((/)h), 



(15) 
(16) 



where gipNOiSE) = E | \ p{n{ki)) ) y Assuming pnoise{j^) to be differentiable on its support [LL, UL] and that 
p(f/L)=p(LL),wehave,E{^^}=0. 

Similarly one can simplify the equations for different combinations of k and m and rjk and 77^ to obtain the 
result F = g{pNOiSE)FG, where the entries are given as follows (see p7|). 

For rjk ur^ and 7/^ i/i^^, we get 



gip noise) < 



For 77j[^ — ix/^ and 77^ — i^/^ we have, 



Fkr, 



g{p noise) < 



cos^(0A,i) if A; = m, 

iGA/'(/c) 

-cos2((/)A;m) if m G A/'(A:), 

o.w. 



sin^(0/ei) if A; = m, 

iGA/'(/c) 

-sin2(0fc^) if m G A/'(A:), 

o.w. 



(17) 



(18) 



For rjk^UR^ and rjm uj^ we have. 



Fkm g{PNOISE) < 



sin((/)/ei) cos((/)A;i) if A: = m, 

iGA/'(/c) 

- sin(0/e^) cos((/)/em) if m E A/'(A:) , 



(19) 



o.w. 



The g{pNOiSE) term can be factored out to get the desired result. 



For a mixture distribution, we have g{pNOiSE) = ^{IploI^{^^^ assumption on pnoise 

holds for a wide class of distributions such as ex-gaussian, gaussian mixtures, uniform distribution etc, that are 
commonly used models in the NLOS setup. The assumption on the identical statistics of the readings is mainly 
questionable when we consider measurements from anchors like GPS and measurements from other neighboring 
vehicles which can have widely different noise parameters. In such a case, one can easily show that the Fisher 



Matrix can be written as F = g{p 



veh 



?veh 



NOISE J G 



+g{p 



sat 

NOISE)"- G 



Fq'^, wherein F^^ is the Fisher Matrix derived by only 
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l/g(p^™p,) as a function of a, a = 0.2m, a = 3m, ^ = 3m g'(a) as a function of alpha, a = 0,2m, a = 3m, = 3m 





Ex - Gaussian 










© Gaussian mixture 
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a - fraction of LOS measurements a - fraction of LOS measurements 

(a) Variation of — r as a function of a for ex-gaussian and (b) a' (a) as a function of a for ex-gaussian. 

Gaussian mixture distributions. 

Figure 5. Behavior of the CRLB as a function of the different noise parameters. 



using the inter-vehicular measurements and Fq'^ is derived by only considering the satellite-vehicle measurements. 
To analyze the behavior, we focus on distance measurements and consider a simple mixture model for the noise 



distribution. We will assume that plos ^ ^/'(O, cf\os)' ^(^) shows the behavior of g{pNOiSE) as a function 
of a for pnlos being ex-gaussian and positive mean gaussian having the same mean and variance. The scaling 

2 

is compared against ^ which seems to be a good approximation of the behavior at higher values of a. At lower 
a values, ^ would be worse off given that we are completely discarding the NLOS measurements. However, 
the ex-gaussian curve performs better given that we make use of the noise statistics. Fig |5(b)| shows a plot of 
g\a) = ^9{-)' The interesting trend to note here is that the differential change in g{.) varies sharply at lower 
values and higher values of a. This suggests that at lower values of a, even a small fraction of LOS measurements 
can significantly help and at higher a a small fraction of NLOS can significantly hurt. 



To analyze the effect of the number of nodes on the localization error, we focus on the case, 6{km) = \ \u{k) — 
u{m)\\. We undertake an incremental analysis by starting out with an existing network of N agents and M anchors 
with Fisher Matrix F = g{pNOiSE)FG and quantifying the effect of adding additional anchors and agents in the 
network. We assume that the node density is large enough and ignore any boundary effects in our derivations. 
Suppose we randomly deploy additional M new anchors and F is the new Fisher Matrix, we have that. 

Theorem 2 ( Scaling law for anchors:). For large M, the lower bound on the sum mean squared localization error 

2N ^ 

is given by, Trace{F~^) — ^(p^^^^^) where p — nB? with pM being the average number of new 

i=i K H 2~ 

anchor measurements of every node and Xi's are the eigen values of Fq (assumed to be full rank). 
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Proof: We provide a sketch of the proof here with a simple example and defer the detailed proof to Appendix 
A. Consider the case where we add a single additional anchor a in a network of N agents and M anchors. Let 
F be the new Fisher Information Matrix. Then it is easy to see that F is the same as F except for the following 
entries. If node k is in the communication radius of a, then we have, for rjk = ur^, F^^ = Fkk + cos^(0fca), for 
r]k = ui^, Fkk = Fkk + sin^(0/ca) and for rjk = UR^.rjm = ^/,, hm = Fkm + ^o^iM sin(0fca) where 0/,^ is the 
angle between node k and anchor a. For all other entries, Fj^j^ = Fj^j^. Thus when M anchors are randomly added 
into the network, the new Fisher Information Matrix has the following structure. 



F = F + 



^ COS^(01a) 








^ COs(0ia) Sin(0ia) 






























^ COS^ (0iVa) 








^ COs(0iVa) sin(0iva) 
aeA/'(Ar) 


COs((/)ia)sin((/)ia) 






































^ COs(0iVa) sin(0iva) 
aeM{N) 








^ sin^ (0iVa) 

aeJ\f(N) 



Since the anchors are randomly placed, it is reasonable to assume that the angles <pka are i.i.d. U (0, 27r) for each 
node k. Thus it is easy to see that for large M the above expression converges to F -> F + ^hN^ where pM is 
the average number of newly added anchors that are neighbors of each node assuming a homogenous placement 
of the nodes. Thus Trace = J^'f^i — where are the eigen values of F. ■ 

The eigen values of the Fisher Information Matrix, {Xi}f^^ can be interpreted as a measure of the "precision" in 
the agent location estimates. The factor ^ is the "additional precision" from the newly added anchors. If under 
the same setup we add N new agents which only get measurements from the existing nodes in the network, then. 

Theorem 3 (Scaling law for agents:). For large TV, the Fisher Information Matrix F is given by. 



F = F + 



pN 



9{pnoise) 2 



1 - 



p{N + M) 



hN - 



p{N + M) 











(21) 



Proof: As before we provide a sketch of the proof with an example and defer the detailed proof to Appendix 
B. Consider the case when N agents are added to an existing network of N agents and M anchors. We will assume 
that the newly added nodes get measurements only with the existing N nodes in the system that are within their 
communication radius. This simplifying assumption helps get a better understanding of the scaling behavior. We 
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will provide an intuition for the derivation using an example here and refer the reader to p7| | for more details. Let 
us suppose that we add a single additional agent to the existing network of agents and anchors. Let the location 
of this agent be zri + jzji. Further assume that this agent gets measurements from all the existing agents in the 
network. Define the new parameter vector as = [ur; u/; zri; zh]. One can verify that the new Fisher Information 
Matrix, F has the following form, 
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(22) 



where Cji = cos(0ji) and Sji = sin(0ji) and 0ji is the angle between the newly added agent and node j. The ^ 
notation is used to distinguish the angles formed due to the newly added agents as opposed to the existing agents in 
the system. If the N nodes are randomly placed, then the angles are ?7(0, 2ti). Thus one can see that the following 
entries converge by strong law of large numbers for large N, i.e., XljLi ^^^^(^ji) ~^ T' XljLi sin^((/)ji) y 
and XljLi cos(0ji) sin((/)ji) 0. Now let us suppose that we add another agent with location Z2 in the network. 
In order to look at positioning error of the first N nodes, we need to look at the Schur Complement of the leftmost 
N X N submatrix of F. The Schur complement of this submatrix is given by F + G — j^BB^, where for the 
parameter ordering fj = [ur; u/; zri;zr2; zh; zn], the matrices B and G are given by. 
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Thus when we add N additiona 



nodes into the system, assuming that the nodes are uniform randomly placed, 








. Extending this to the case when each additional node has 

measurements only with a fraction ~p of the existing nodes and anchors as well the above derivation holds with N 
replaced by p{N + M) and N replaced by pN, ■ 

Corollary 1. For large N, assuming F to be full rank, the above bound reduces to, Trace{F~^) = ^(j^^^^^^) ^2^1=1 

The result quantifies the benefits of co-operation between agents. The agents can be interpreted as virtual anchors 
in the network that help localize the other nodes in the system. 

Now let us consider the setup where the agents are mobile and we have some estimates of their velocities at each 



time instant from their INS readings. The vector of parameters is taken as follows 77 
where ut is the vector of vehicle locations at time t and we consider T time instants. Assuming that each reading 
6t{km) is sampled independently from the noise distribution Pnoise{-) and that the velocity measurement St{m) 
is given by, st{m) = ut{m) — ut-i{m) + wt{m)^ where wt{m) ~ A/'(0, crjj^g), the following theorem holds true. 

Theorem 4 (Mobile case:). The Fisher Information Matrix F is given by F = g{pNOiSE)FG + -^^FiNS^ under 
the assumption that Pnoise is differentiable over its support [LL^ UL] and p{UL) — p{LL) = 0. 

Proof: Appendix C. ■ 
Fq is a matrix that only depends on the node locations at different time instants (see Appendix D) and Fjns is 
a constant matrix. 



VI. Simulation results 

The simulation setup consisted of vehicles on a four lane road with lane width as 4m and lane length of 100m. 
Eight vehicles were simulated in each lane moving with an average velocity of SOmph. Two adjacent lanes had 
vehicles moving in one direction and two others had vehicles moving in the opposite direction. The vehicles do 
not change their lanes. Vehicles broadcast messages every 100ms (in accordance with the DSRC standard). Each 
message contains the vehicle position estimate and an estimate of the variance in its location estimate. In addition. 
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Plot of satellites at different elevations viewed from Lat Lon 
at GPS Time 568800 (Sat 2pm) 



North 
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deg elevation (horizon^ ^ 




East 



South 



Figure 6. Satellite positions at different elevation angles viewed from deg Lat and deg Lon at GPS time 568800s (Saturday 2pm). The 
different rings are increasing elevations of 10 degrees with the outermost ring corresponding to the horizon at deg. 

vehicles also get ranging estimates with their neighbors which can be obtained using short probe packets that are 
typically a few symbols long. 

Unless specified otherwise, the following parameters are used for the simulations. The radius of communication 
is assumed to be 50m (typically 50m- 100m is used in these applications). GPS satellites are used as anchors. The 
mask angle for each vehicle at every time instant is taken to be uniform in the range 55 degrees to 85 degrees. 
This gives an average visibility of one satellite over every two time instants for each vehicle. Satellite locations 
are simulated from their almanac corresponding to the GPS time of week 568800s (UTC, Saturday 2pm). Fig. [6j 
shows the positions of different GPS satellites as seen from deg Lat deg Lon position for a deg mask angle. 
The different rings correspond to different elevation angles spaced 10 degrees apart with the outermost ring being 
degrees. The noise model is taken the be same as in section |IV| The LOS thermal noise standard deviation in the 
GPS readings is taken as 10m (no ionospheric/tropospheric biases assumed). The standard deviation in the NLOS 
noise is taken to be 5m to capture nearby reflectors such as other vehicles and far away reflectors like buildings on 
the roadside. The INS noise is assumed to have a standard deviation of Im/ s [ [35| . The algorithm was initialized 
with 2000 particles for each vehicle. In all the plots that follow, we will show three curves. One that corresponds to 
the mean performance of the algorithm with the 90 percentile error bars. The second curve is the CRLB evaluated 
given all the measurements. The third bound is the "causal" CRLB computed by considering for parameters at each 
time instant t (i.e. {ut{k)}^^^ only the measurements until time t. The y-axis in all the plots is the localization 
error (m). 
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Localization Error as a function of the l\/lasl(Angle 
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Figure 7. Performance of the algorithm as a function of the mask angle (number of satellites seen), asat = 10m,aR = 2m, ctins = 5m/ s, 
a = 0.5, radius of communication = 10m ^ 5 neighbors. 



Localization error as a function of thie thiermal noise in the ranging measurements 




LOS thermal noise in the ranging measurement (m) 

Figure 8. Performance of the algorithm as a function of the ranging noise, asat = 10m, aiNS = Im/s, a = 0.5. 



Fig. [7] shows the error performance as a function of the mask angle (equivalently number of visible satellites) 
as compared against a standard least squares GPS receiver algorithm which is significantly worse off at lower 
satellite visibility mainly due to the NLOS nature of the measurements. In this simulation, we also simulated the 
Galileo satellite constellation to have a larger dynamic range of visible satellites. Fig. [8] shows the error performance 
as a function of the thermal noise in the ranging measurement between vehicles. At very low values of aR, the 
performance of the algorithm is close to the lower bound. However as aji increases, the performance quickly 
deviates from the bound though it follows the scaling trend without diverging. 
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Error as a function of the fraction of LOS readings 




Fraction of LOS signals (a) 



Figure 9. Performance of the algorithm as a function of the fraction of LOS measurements, a sat — 10m, aR = Im, (Jins = Im/s. 



Localization error as a function of the thermal noise in the INS measurements 




23456789 
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Figure 10. Performance of the algorithm as a function of thermal noise in the INS measurements, asat = 10m, an = 3m, a = 0.5. 



Fig. |9] is a plot of the error performance as a function of the fraction of LOS measurements. The errors are 
seen to be quite reasonable over a wide range of a. As before, the performance is close to the bound at higher 



signal-to-noise ratio i.e. large a. Fig. [TO] shows the performance of the algorithm as a function of the measurement 
noise in the INS readings. Clearly the performance diverges from the bound as the measurement noise becomes 
large. This is an inherent limitation of the algorithm given that each vehicle uses an estimate of the neighbor's 
location extrapolated using only the INS measurements. Given that the position has to sustain until enough LOS 
measurements are gathered, a larger INS error cannot be tolerated by the algorithm. The hope is that the INS 
measurement error is small enough in practical systems for the algorithm to work. Vehicle INS sensors can have 
error < Im/s (351 which can be tolerated by the algorithm. 



Fig. 11 is a plot of the performance of the algorithm as a function of the communication radius. Essentially we 
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Localization error as a function of the communication radius 




45 50 



Radius of communication (m) 

Figure 11. Performance of the algorithm as a function of the radius of communication, asat = 10m, an = Im, ctins = Im/s, a = 0.5. 



are interested in seeing the performance improvement as a function of the increase in the number of neighbors of a 
node. As expected there is diminishing returns with increased radius. Having a large communication radius is good 
for localization. However this leads to decreased spatial reuse of the spectrum, thereby leading to a tradeoff. One 
could think of an adaptive system wherein the vehicles increase the communication radius in harsh environments 
like urban canyons while reduce it in open sky environments where the number of visible satellites is larger. 



Ranging thermal Noise (m) Average N^ff 



1 84.3 

2 162.2 

3 245.1 

4 306.3 

5 402.2 



Table I 

Effective number of particles (Neff) as a function of the ranging noise 



Particle filtering inherently has the problem of degeneracy where the number of distinct particles reduce with 
time. Thus it is important to understand the evolution of distinct particles for the problem setup. We used 2000 
particles to start with and the effective number of particles is around 100 depending on the parameters of the 
simulation. We set a threshold of 30 before we resample. Table [l| and Fig. 12^how the variation of N^ff as a 
function of different parameters. For a larger noise in general, N^ff is larger as expected since more number of 
particles will have a significant weight given the larger uncertainty in the position. However clearly the error is also 
larger. A smaller noise gives better error performance but the degeneracy problem kicks in earlier which would later 
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Evolution of N^^^ with time starting out with 2000 particles 

160 1 1 1 1 1 1 1 1 




20 1 1 1 1 1 1 1 1 1 

10 20 30 40 50 60 70 80 

Time Steps (x 0.1s) 

Figure 12. Effective number of particles (Neff) as a function of time. 



adversely affect the error performance. N^ff in Fig. 12 increases over time since, after resampling we would have 
a larger number of distinct particles closer to the mean value and thereby each of them will have a significant weight. 



Convergence performance 



1.2 




5 10 15 20 25 30 35 40 45 50 
distance in m 

Figure 13. Error convergence as a function of distance. 

Given the approximate nature of the algorithm, it is likely that the algorithm can diverge for certain parameters 
of the problem setup. Hence it is important to see how well the errors converge and when would the performance 
diverge. Fig. [13] shows the error convergence as a function of the distance (equivalently time steps) for aR = Im, 



asat = 10m, ajNs = Im/s, a = 0.5 and radius of communication = 50m. Fig. 13 shows the error as a function 
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Localization error as a function of the distance 
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Figure 14. Algorithm diverges when the noise is large. 

of the distance for the case when aR = 3m, ctins = lOm/s. Clearly the algorithm diverges for a larger noise in 
the INS readings. This is due to the inherent nature of the algorithm, since each vehicle extrapolates the neighbor's 
location using the INS measurements and uses that to compute its own belief. Thus a larger INS noise can lead to 
divergence. 

VII. Conclusion and Future work 

In this work we explored the application of particle filtering to get estimates of vehicle locations in a highly 
NLOS environment. We derived weight update equations for the NLOS setting and simulation results show that 
reasonably good accuracies in positioning is feasible. The approximation in the graphical model could break down 
above a certain noise threshold and below a certain anchor density, and the algorithm could potentially diverge. A 
theoretical understanding of when the algorithm diverges is another research direction, though we believe this to 
be a hard problem. We also explored the behavior of the localization error as a function of the number of anchors, 
vehicles and the fraction of LOS measurements by analyzing the CRLB. The performance of the proposed algorithm 
was evaluated in comparison to the derived bound and was shown that the algorithm respects the scaling behavior 
as predicted by the bound. One can see that the errors converge in around 10 steps. 
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Recall that u is the vector of agent/vehicle locations. We will treat the anchors separately and will denote the 



difference, Xi — xj, between any two nodes can be described using the x M vector Cij, whose zth entry is 1 
and jth entry is -1 and all other entries are set to zero. Thus we get, 



Appendix A 



Derivation of the CRLB for Anchors 



u 



G C^"^^ be the vector of all the node locations. The location 



vector of anchor locations by v. Let x = 



V 




(25) 



Let L be the total number of distance measurements obtained in the network. We will assume that distance 
measurements are obtained between nodes that are within a communication radius R of each other. Collecting 
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all the location differences we get the following relation, 



(26) 



where the rows of E e ^Lx(n+m)^ ^^iq vectors efj and y G C^^^, is a complex vector of all the available 
location differences of nodes that are within a radius R of each other. The absolute value of each entry in the 
vector y denotes the distance between the corresponding two nodes obtained from the matrix E. 



Let E = [E1E2], where Ei G 



pLxN 



and E2 G 



pLxM 



. We can then write, 



Eiu + E2\' = y. 



(27) 



Let d denote the vector of all distances between nodes having observations i.e. d = |y|, where |y| is a notation 
used to denote a vector whose components are the absolute values of the individual components of y. Let d denote 
the vector of pairwise distance measurements i.e. 



dj = dj + no, 



(28) 



where nj ^ Pnoise{-)' Define the two real diagonal matrices. 



Dr = i?e{diag{^i/|yi| 

Di = /m{diag{^i/|yi| ^l/I^lI}}. 



(29) 
(30) 



The authors in p6| obtain a compact representation for the Fisher Information Matrix for LOS Gaussian noise as 
shown below. 



EfDlEi EJDrDiEi 
EJDrDiEi EjD]Ei 

F is assumed to be invertible. In our generalized case we get, 

EjDlEi EJDrDiEi 
EIDrDjEi EfDjEi 



(31) 



giPNoiss) 



(32) 



Since we are interested in analyzing the behavior as a function of the number of anchors, we will ignore the 
scalar ^(p^^^^^) ^nd work only with the matrix which we denote as Fq. Let us suppose that we add a single anchor 
to the set of existing nodes. Let / be the number of additional distance measurements that are obtained. Without 
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loss of generality assume that the first I nodes get measurements with respect to the new anchor. The equation 
relating the agent locations to the location differences, Ex — y now gets updated to, 



Ei\ E2 
I;|0 -1 



X 

XN+M+l 



y 

VL+l 
VL+l 



(33) 



where 1/ is the identity matrix of dimension I, and 1 are vectors/matrices consisting of all zeros and all ones 
respectively, of appropriate dimensions. Let Ai = [li\0]. Define 



Dr^ = Re{diag{yL+i/\yL+i\ yL+i/\yL+i\}} 

Di, = 7m{diag{yL+i/|?/i+i| yL+i/\yL+i\}} 



(34) 
(35) 



Thus, after adding a single anchor, the matrix Ei gets updated to 



Matrix get updated as follows. We have EfDj^Ei getting updated to 
Ai = [I/|0], the new Fisher matrix can be written as. 



El 
li\0 



. Hence the entries of the Fisher 



. Defining 



El 
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El 


I/|0 











I/|0 



Fg 



Fg + 



(36) 



AjDlAi AJDr,Di,Ai 
AJ Dr^Di^ Ai AfD^Ai 

Note that in this case we defined a particular ordering of the nodes, i.e. the first I nodes to have measurements 
with the newly added anchor. For a general ordering, it is easy to see that the identity matrix in the definition of 
Ai would be replaced by a general permutation matrix. Thus, if we add anchors recursively in the network, 
the new Fisher matrix can be expressed as. 



Fg = Fg + Y^ 



(37) 



AJDr^DiAi AfDjA, 

Here the matrices A^ of size N x k where k nodes get measurements with the ith newly added anchor, have 
ones corresponding to the columns of the nodes with which the ith newly introduced anchor gets measurements. 
Dr. , Dj. have definition similar to Dr^ and Dj^ . For simplicity lets first consider the case where the newly 
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introduced anchors have measurements with all the agents. We then have = 1^ Vi giving us, 



1=1 



(38) 
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M' 
i=l 


M' 
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M' 
1=1 


M' 


Y,DUj)DR,{j) 
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M' 

1=1 


COs(0ij) 



By definition, DR.{j) = where k = L + {i - 1)N + j. This can also be equivalently written as DR.{j) = 

cos{(f)ij) where (pij is the angle made by the line joining ith newly added anchor node and the jth agent, with the 
horizontal axis. Let us assume that each anchor that is newly introduced is randomly placed in the field independent 
of all other nodes. Thus for each node j, {(f>ij}i£iS are i.i.d and distributed U(0, 27r). Similarly Di.{j) — sin(</)jj) 
and by the strong law of large numbers we get, 

ML n^' 

(39) 
(40) 

0. (41) 

(42) 

The Fisher matrix now simplifies to, 

M' 

Fg = Fg + —I2N w.h.p. (43) 

We know that the Fisher Information Matrix is a covariance matrix and hence is symmetric positive definite. We 
can write Fq = UAU^, where A is a diagonal matrix of the eigen values of Fq and UU^ = U^U = I. We also 
know that Tr{ABC) = Tr{CAB) = Tr{BCA). This gives us, 

Trace(FG~^) = TraceiUAU" + ^I2n)~\ (44) 
= Tmce{U{A + —l2NW)-\ (45) 

2N 

= E^T^- (46) 

1=1 + ^ 

It is now easy to extend the analysis to the case where each anchor node has measurements only with the nodes 
that are within a radius R. Let A be the total area of the field where the nodes are placed. Let p = and then 
pM' would be the average number of neighbors of each node. In this case we would have Af D| ^ ef^/jv 
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and so on. One can easily show that, 



Trace(Fa"') = ^ 



(47) 



2=1 + ^ 

Appendix B 
Derivation of the CRLB for agents 

The setup is similar as in the previous case with N agents, M anchors and L distance measurements. We are 
interesting in characterizing the behavior of the CRLB after adding N' agents to the existing network of anchors 
and agents. Consider the simple case when N' = 1. We will assume for now that the newly introduced agent has 
measurements with all of the existing N agents and M anchors. Let Fq, E, Ei, Dr, Dj, be the new set of matrices 
obtained after adding this node. We then have the following. 



E = 



El Ol E2 
-In In 
On 1m —Im 



(48) 



Let 



Ei = 



El Ol 
-In Itv 
On 1m 



(49) 



Dr^^ = J?e{diag{yL+i/|yL+i| yL+Ar/|?/L+7v|}} 

I^Ri2 - Re{diag{yL+N+i/\yL+N+i\ Vl+n+m /\yL-^N-hM\}} 

Dill - Im{dmg{yL+i/\yL+i\ vl+n/Ivl+nI}} 

Di^^ = /m{diag{?/L+iv+i / 1 Vl+n+i \ Vl+n+m / \ Vl+n+m \ } } 



(50) 
(51) 
(52) 
(53) 



Then 



Dr = 



Dr 
Dr,, 
Dr,, 



(54) 
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Di 

Di= D/,, 

Di^, 

The new Fisher Information Matrix is given by, 



(55) 



EJDrDiEi EfDjEi 



(56) 



The individual terms of Fq can be simplified as shown in ( 75 ) - ( 80 ) (lengthy equations are in the last page) 
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(58) 



The terms in the new Fisher Information Matrix can be simpUfied and have the structure shown in (80). The Fisher 



matrix evolution as more and more nodes are added is apparent from the expression ( |80| ). Similar evolution holds 
for other block terms in the Fisher matrix. To simplify the analysis it would be good if we could separate out the 
original Fisher Information Matrix terms and express Fq in terms of Fq. This requires rearranging some of the 
terms in Fq. Recall the definition of Fq. We had 77 = [u^Uj']'^, where ur = i?e{u}, ui = Im{u}. Then Fq is 
given by, 

(df{d\v) df{d\v)\ 



Eg., = E 



(59) 



I drji drjj j ' 

Let z = zr + jzi G C^^^, denote the location of the newly added nodes. The Fisher Information Matrix, Fq that 
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we have calculated corresponds to the following ordering of the parameters, 
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Ur Ui 
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We will now rearrange the parameters so as to get, 

Ur 
ui 
zr 
zi 

Retaining the same notation for Fq, we get the following simplification. 



Fc + Aii Ai2 
A21 A22 



where. 



A 



11 



(60) 
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(63) 



A22 and A12 are given by the expressions (82) and (183]) respectively. 



We are now interested in the error improvement of the first agents after the addition of A^' agents. For this, 
it is sufficient to look at the Schur Complement of the matrix A22, since the inverse of the Schur Complement 
corresponds to the CRLB restricted to the first A'^ nodes. The Schur Complement is given by, 

F + Aii-Ai2A^2^A2i. (64) 

Based on similar arguments as in the case of anchor nodes, assuming that each of the newly added nodes are 
distributed uniform i.i.d into the network, we have for large A'^', 

A^' 

All -^^hN- (65) 

Let us now assume that the initial set of nodes were also placed uniform randomly in the field. Consider the terms 
in A22- Each of the terms l^D\^^ljq are the sum of the cosine of the angles made by the newly introduced jth 
node with all the existing nodes in the network. Under the random placement assumption, these angles can also be 
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taken to be distributed i.i.d ?7(0, 27r). Thus we have for large N, 

A22 — '-^ — hN'- (66) 

Note the difference in this approach as compared to that for the anchor nodes. Here we are averaging over all initial 
node placements also for this approximation to hold. For the anchor nodes, the result was true for any initial node 
placement. 



With the above approximation in place, we have 



Ai2A22^Af2 



N + M 



:Ai2Af2. 



(67) 



This can be expanded to obtain the expression ( |84| ). With the usual law of large numbers argument, each of the 
terms in the matrix converge as grows large. The corresponding values to which the kith term in the matrix 
converges are shown in ([86l)-([9T]). Thus we get. 
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(68) 
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The Schur complement can now be simplified as shown in (92). The CRLB restricted to the first A'^ nodes is given 
by the inverse of this Schur Complement. Now consider the case when measurements are obtained only between 
nodes that are within a radius R of each other. Let p — then similar arguments would simplify the new Fisher 
matrix F' to be, 
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(69) 



Appendix C 
Mobile setting 

Let u/i,+ju/, be the vector of vehicle locations at time t. Define the parameter vector as 77 = 
Let s be the vector of velocity measurements. Since the measurement noise is independent across the velocity 
measurements and the ranging measurements, it is easy to see that the (fc, m)th entry in the Fisher Information 
Matrix is given by, 



^ ]g / ^lnp(e|??)ainp(e|??) ) ^ ^ f dlnp{ s\r]) dlnpj s\7]) 



(70) 
(71) 
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Define the matrix Fq such that, 



E 



dlnp{e\ri)d\np{e\ri)\ 



drik 



drim j ' 



(72) 



and matrix Fjns such that, 



dlnp{ s\ri) dlnp( s\ri) 



(73) 



dm drjm 

Fq is the Fisher Information Matrix corresponding only to the ranging measurements and Fjns is the Fisher 
Information Matrix characterizing the contribution of the inertial navigation system measurements to positioning. 
Consider the matrix Fq. Since the measurement noise is independent across vehicles as well as across time, one 
can verify that, for {m = UR^^r]^ = ur^,), we have that Fg^^ = whenever t ^ t' . Similarly this holds true for 
other pairs of parameters {rjk^rjm) that corresponds to different time instants. Thus, we would end up with a block 
diagonal structure of the following form. 



Fail) 

Fg{2) 







(74) 







Fg{T) 

where Fcit) is the Fisher Information Matrix as derived in Appendix A, with the vehicle locations at time t. Fjns 
can be evaluated as follows. Due to the independence of noise, the only non-zero terms in the matrix would be the 
diagonal and the first off-diagonal entries. One can verify that for rjk = UR^{k), rjm = UR^{k), FjNSkm — for 



Vk = UR^{k), r]m = UR^_^{k), FiNSkm = 
this holds for the imaginary components. 



and 



for r]k UR^k), r]m = UR^^^{k), FiNSum = Similarly 
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(84) 
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